// Robot klasse
// Sam De Kunst

#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <iostream>

#include <wiringPi.h>
#include <wiringSerial.h>

#include "robot.h"
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Robot() --> create robot element
//		- check if connection is avaible, else print error
//		- connect to robot, if not able print error
// Robot::sentCmd --> basic function to send commands as string to the robot
//		- take argument and sent to robot
//		- close with an enter (13)
// Robot::getDistance --> get distance from front sensor
//		- get the value from the sensor by sending the command 'u'
//		- in a loop, catch all send chars from robot and store in response
//		- response will be in format off "Sharp value = [value]"
// 		- check if format is correct, and return value as INT, else trow error
// Robot::setSpeed --> set the speed of the motors of the robot
//		- set the speed by sending the command 'q' followed by twice the speed argument (one for left an one for right)
// Robot::rotate --> let the robot rotate
//		- we use the same command as setspeed, but one argument will be the negative off the other
// Robot::servo --> open or close the claws
//		- use command 'g' followed by INT, 0 for open, 1 to close
//Robot::servoOpen --> open the claws with command 'g' followed by 0
//Robot::servoClose --> close the claws with command 'g' followed by 1
//Robot::stop	--> stop all movement of the robot
//		- send 'q' command with both arguments '0'
//Robot::destroy --> delete robot element	
//		- call stop() function and close the serial connection
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void Robot::sendCmd ( char cmd[] ) {
	serialPuts(fd, cmd);	// send cmd to pi
	serialPutchar(fd, 13);	// terminate cmd
}

Robot::Robot(){
	std::cout << "Robot created\n";
	if ((fd = serialOpen ("/dev/ttyUSB0", 38400)) < 0){
		fprintf (stderr, "Unable to open serial device: %s\n", strerror (errno)) ;
	}

	if (wiringPiSetup () == -1){
		fprintf (stdout, "Unable to start wiringPi: %s\n", strerror (errno)) ;
	}
}

int Robot::getDistance(void){
	// clear the remaining data
	serialFlush(fd);
	delay(200);

	// send the new data 
	char cmd[2] = "u";
	serialPuts(fd, cmd);	// send cmd to pi
	serialPutchar(fd, 13);	// terminate cmd

	// get the response
	char response[20];
	int dist;
	int i= 0;
	
	delay(200);
	while (serialDataAvail (fd) )
		response[i++] = serialGetchar(fd);
	//printf("Robot::getDistance() : %s", response);

	if (strncmp(response, "Sharp value = ", 13) == 0){
		sscanf(response, "%*s %*s %*s %d", &dist);
		return dist;
	}
	else
		return -1;
}

void Robot::setSpeed( int speed_){
	char cmd[15]; 									// command sent to the pi later			
	sprintf(cmd, "q %d %d", speed_, speed_);
	sendCmd(cmd);
}

void Robot::rotate( int rotationSpeed){
	char cmd[15]; 									// command sent to the pi later
	sprintf(cmd, "q %d %d", rotationSpeed, -rotationSpeed);
	sendCmd(cmd);		
}

// :servo(0) = open, :servo(1) = close
void Robot::servo ( int close ){						
	char cmd[15];
	sprintf(cmd, "g %d", close);
	sendCmd(cmd);		
}

void Robot::servoOpen(void){
	return servo(0);
}

void Robot::servoClose(void){
	return servo(1);
}

void Robot::stop(){
	char cmd[15];
	sprintf(cmd, "q 0 0");
	sendCmd(cmd);		
}
void Robot::destroy(){
	printf("Closing the serial connection, bye bye!\n");
	stop();
	serialClose(fd);
}
